/****************************************************************************
 *
 *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file heading_fusion.cpp
 * Magnetometer fusion methods.
 * Equations generated using EKF/python/ekf_derivation/main.py
 *
 * @author Roman Bast <bapstroman@gmail.com>
 * @author Paul Riseborough <p_riseborough@live.com.au>
 *
 */

#include "ekf.h"

#include <ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h>
#include <ekf_derivation/generated/compute_mag_y_innov_var_and_h.h>
#include <ekf_derivation/generated/compute_mag_z_innov_var_and_h.h>

#include <ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h>

#include <mathlib/mathlib.h>


// 磁力计测量融合方程
bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states, bool update_tilt)
{
	// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
	const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));  // 噪声协方差矩阵

	// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains
	Vector3f mag_innov;  // 新息向量
	Vector3f innov_var;  // 新息方差

	// Observation jacobian and Kalman gain vectors
	VectorState H; // 观测雅可比矩阵
	const auto state_vector = _state.vector();
	sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);

	// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
	if (_control_status.flags.synthetic_mag_z) {
		mag_innov(2) = 0.0f;  // 如果磁力计是与GPS融合的，则不使用Z轴测量数据，直接赋予z轴新息为0，不进行更新
	}

	for (int i = 0; i < 3; i++) {
		aid_src_mag.observation[i] = mag(i) - _state.mag_B(i);
		aid_src_mag.observation_variance[i] = R_MAG;
		aid_src_mag.innovation[i] = mag_innov(i);
		aid_src_mag.innovation_variance[i] = innov_var(i);
	}  // 那个结构体存储相关数据

	const float innov_gate = math::max(_params.mag_innov_gate, 1.f);
	setEstimatorAidStatusTestRatio(aid_src_mag, innov_gate);  // 新息太大或发散掉，reject

	if (update_all_states) {
		// 新息的方差比设定的磁力计的方差小，则认为磁力计的数据不够精确，改变标志位
		_fault_status.flags.bad_mag_x = (aid_src_mag.innovation_variance[0] < aid_src_mag.observation_variance[0]);
		_fault_status.flags.bad_mag_y = (aid_src_mag.innovation_variance[1] < aid_src_mag.observation_variance[1]);
		_fault_status.flags.bad_mag_z = (aid_src_mag.innovation_variance[2] < aid_src_mag.observation_variance[2]);

	} else {
		_fault_status.flags.bad_mag_x = false;
		_fault_status.flags.bad_mag_y = false;
		_fault_status.flags.bad_mag_z = false;
	}

	// Perform an innovation consistency check and report the result
	_innov_check_fail_status.flags.reject_mag_x = (aid_src_mag.test_ratio[0] > 1.f); // test_ratio 在上面 setEstimatorAidStatusTestRatio 函数中进行的计算  新息太大直接拒绝
	_innov_check_fail_status.flags.reject_mag_y = (aid_src_mag.test_ratio[1] > 1.f);
	_innov_check_fail_status.flags.reject_mag_z = (aid_src_mag.test_ratio[2] > 1.f);

	const char *numerical_error_covariance_reset_string = "numerical error - covariance reset";

	// check innovation variances for being badly conditioned
	if (innov_var.min() < R_MAG) {  // 坏情况，重置姿态和磁场相关的（因为三轴磁力计的测量跟他们有关）
		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
		// we need to re-initialise covariances and abort this fusion step
		if (update_all_states) {
			resetQuatCov(_params.mag_heading_noise);
		}

		resetMagCov();

		ECL_ERR("mag %s", numerical_error_covariance_reset_string);
		return false;
	}

	// if any axis fails, abort the mag fusion
	if (aid_src_mag.innovation_rejected) { // 新息太大或发散掉，直接拒绝， setEstimatorAidStatusTestRatio 返回值
		return false;
	}

	bool fused[3] {false, false, false};  // 三轴是否融合成功的标志

	// update the states and covariance using sequential fusion of the magnetometer components
	for (uint8_t index = 0; index <= 2; index++) {
		// Calculate Kalman gains and observation jacobians
		if (index == 0) {
			// everything was already computed above
			// 不用重新计算新息和新息方差

		} else if (index == 1) {
			// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
			sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H);

			// recalculate innovation using the updated state
			aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);

			if (aid_src_mag.innovation_variance[index] < R_MAG) {
				// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
				_fault_status.flags.bad_mag_y = true;

				// we need to re-initialise covariances and abort this fusion step
				if (update_all_states) {
					resetQuatCov(_params.mag_heading_noise);
				}

				resetMagCov();

				ECL_ERR("magY %s", numerical_error_covariance_reset_string);
				return false;
			}
		// 前面ComputeMagInnovInnovVarAndHx函数一次性计算出了三个轴的新息和新息方差，而在x轴的循环中已经对状态向量和协方差矩阵P更新了，所以y轴和z轴的计算需要重新进行
		} else if (index == 2) {
			// we do not fuse synthesized magnetomter measurements when doing 3D fusion
			if (_control_status.flags.synthetic_mag_z) {
				fused[2] = true;
				continue;  // 新息为0，没必要融合了
			}

			// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
			sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H);

			// recalculate innovation using the updated state
			aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);

			if (aid_src_mag.innovation_variance[index] < R_MAG) {
				// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
				_fault_status.flags.bad_mag_z = true;

				// we need to re-initialise covariances and abort this fusion step
				if (update_all_states) {
					resetQuatCov(_params.mag_heading_noise);
				}

				resetMagCov();

				ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
				return false;
			}
		}

		VectorState Kfusion = P * H / aid_src_mag.innovation_variance[index];  // Kalman增益

		if (!update_all_states) {  // 函数输入的控制位，如果不是更新所有状态，则只更新磁力计相关的状态，不更新其他状态
			// zero non-mag Kalman gains if not updating all states

			// copy mag_I and mag_B Kalman gains
			const Vector3f K_mag_I = Kfusion.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0);
			const Vector3f K_mag_B = Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0);

			// zero all Kalman gains, then restore mag
			Kfusion.setZero();
			Kfusion.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) = K_mag_I;
			Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
		}

		if (!update_tilt) { // 函数输入的控制位，如果不是更新磁场的俯仰角，则只更新磁力计相关的状态，不更新其他状态
			Kfusion(State::quat_nominal.idx) = 0.f;
			Kfusion(State::quat_nominal.idx + 1) = 0.f;
		}

		if (measurementUpdate(Kfusion, H, aid_src_mag.observation_variance[index], aid_src_mag.innovation[index])) { // 更新了，他直接访问参数的地址
			fused[index] = true;
			limitDeclination();

		} else {
			fused[index] = false;
		}
	}

	if (update_all_states) {
		_fault_status.flags.bad_mag_x = !fused[0];  // 根据有没有融合成功再次设置标志位
		_fault_status.flags.bad_mag_y = !fused[1];
		_fault_status.flags.bad_mag_z = !fused[2];
	}

	if (fused[0] && fused[1] && fused[2]) {
		aid_src_mag.fused = true;
		aid_src_mag.time_last_fuse = _time_delayed_us;

		if (update_all_states) {
			_time_last_heading_fuse = _time_delayed_us;
		}

		return true;
	}
	// 融合成功的上面已经return了，剩余情况就融合失败了，返回false
	aid_src_mag.fused = false;
	return false;
}

// 磁偏角的融合
bool Ekf::fuseDeclination(float decl_sigma)
{
	if (!_control_status.flags.mag) { // 磁力计要有效才能融合
		return false;
	}

	float decl_measurement = NAN;

	if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) // GPS有磁偏角且设置为使用GPS数据（USE_GEO_DECL）
	    && PX4_ISFINITE(_mag_declination_gps)
	   ) {
		decl_measurement = _mag_declination_gps;  // 如果GPS有磁偏角则用GPS的磁偏角测量

	} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) // 设置为使用本地预存的磁偏角数据（SAVE_GEO_DECL）且有效则用本地数据
		   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
		  ) {
		decl_measurement = math::radians(_params.mag_declination_deg);
	}

	if (PX4_ISFINITE(decl_measurement)) {

		// observation variance (rad**2)
		const float R_DECL = sq(decl_sigma);

		VectorState H;    // 线性化的观测函数
		float decl_pred;  // 磁偏角的预测值
		float innovation_variance; // 新息的方差，都由ComputeMagDeclinationPredInnovVarAndH返回，直接访问变量地址把值修改了

		sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);

		const float innovation = wrap_pi(decl_pred - decl_measurement); // 磁偏角的新息，限制在-pi到pi之间

		if (innovation_variance < R_DECL) {
			// variance calculation is badly conditioned
			return false;  // 如果新息方差太小不合理，则返回false，融合失败
		}

		// Calculate the Kalman gains
		VectorState Kfusion = P * H / innovation_variance;  // 卡尔曼增益 P*H*(HPH^T + R)^{-1}，其中 innovation_variance = HPH^T + R

		const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation); // 更新

		_fault_status.flags.bad_mag_decl = !is_fused;  // 如果融合失败，则设置bad_mag_decl标志位，坏数据

		if (is_fused) {
			limitDeclination(); // 成功融合后仍要限制磁场强度的值
		}

		return is_fused;
	}

	return false;
}

// 限制磁偏角的值并根据限制后磁偏角修改状态中的磁场强度
void Ekf::limitDeclination()
{
	const Vector3f mag_I_before = _state.mag_I;

	// get a reference value for the earth field declinaton and minimum plausible horizontal field strength
	float decl_reference = NAN;
	float h_field_min = 0.001f;  // 有效磁场水平分量的最小阈值

	if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL
	    && (PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps) && PX4_ISFINITE(_mag_inclination_gps))
	   ) {
		decl_reference = _mag_declination_gps;  // 还是GPS能用用GPS的

		// set to 50% of the horizontal strength from geo tables if location is known 如果位置已知，则设置为地理查询表中水平磁场强度的50%。
		h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));

	} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
		   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
		  ) {
		// use parameter value if GPS isn't available
		decl_reference = math::radians(_params.mag_declination_deg);  // 用本地预设数据
	}

	if (!PX4_ISFINITE(decl_reference)) {
		return;
	}

	// do not allow the horizontal field length to collapse - this will make the declination fusion badly conditioned
	// and can result in a reversal of the NE field states which the filter cannot recover from
	// apply a circular limit
	float h_field = sqrtf(_state.mag_I(0) * _state.mag_I(0) + _state.mag_I(1) * _state.mag_I(1));  // 计算水平磁场强度，从状态中获得

	if (h_field < h_field_min) {
		if (h_field > 0.001f * h_field_min) { // 如果水平磁场强度小于最小值，但没有太小，根据比例放大到最小阈值
			const float h_scaler = h_field_min / h_field;
			_state.mag_I(0) *= h_scaler;
			_state.mag_I(1) *= h_scaler;

		} else {  // 如果水平磁场强度台效力，那么直接设置成最小值的两倍（根据预设的磁偏角计算）
			// too small to scale radially so set to expected value
			_state.mag_I(0) = 2.0f * h_field_min * cosf(decl_reference);
			_state.mag_I(1) = 2.0f * h_field_min * sinf(decl_reference);
		}

		h_field = h_field_min;
	}

	// do not allow the declination estimate to vary too much relative to the reference value
	constexpr float decl_tolerance = 0.5f;
	const float decl_max = decl_reference + decl_tolerance;
	const float decl_min = decl_reference - decl_tolerance;
	const float decl_estimate = atan2f(_state.mag_I(1), _state.mag_I(0));

	if (decl_estimate > decl_max)  { // 如果估计值大于磁偏角最大值，则使用最大值
		_state.mag_I(0) = h_field * cosf(decl_max);
		_state.mag_I(1) = h_field * sinf(decl_max);

	} else if (decl_estimate < decl_min)  { // 如果估计值小于磁偏角最小值，则使用最小值
		_state.mag_I(0) = h_field * cosf(decl_min);
		_state.mag_I(1) = h_field * sinf(decl_min);
	}

	if ((mag_I_before - _state.mag_I).longerThan(0.01f)) {  // 如果修改之后比修改之前磁场强度变化太大，则打印警告信息
		ECL_DEBUG("declination limited mag I [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f] (decl: %.3f)",
			  (double)mag_I_before(0), (double)mag_I_before(1), (double)mag_I_before(2),
			  (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
			  (double)decl_reference
			 );
	}
}

// 理论计算的磁场强度和磁力计测量z轴磁场强度的融合
float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted)
{
	// theoretical magnitude of the magnetometer Z component value given X and Y sensor measurement and our knowledge
	// of the earth magnetic field vector at the current location
	// 水平磁场强度测量精度比水平磁场测量精度高
	const float mag_z_abs = sqrtf(math::max(sq(mag_earth_predicted.length()) - sq(mag_meas(0)) - sq(mag_meas(1)), 0.0f));

	// calculate sign of synthetic magnetomter Z component based on the sign of the predicted magnetometer Z component
	const float mag_z_body_pred = mag_earth_predicted.dot(_R_to_earth.col(2));

	return (mag_z_body_pred < 0) ? -mag_z_abs : mag_z_abs;
}
